#include <math.h>
#include <algorithm>
#include <assert.h>
#include <limits>
#include <stdexcept>
#include <iostream>
#include <cmath>
#include <stdio.h>
#include "math_commands.h"
#include "core/value.h"
#include "core/inspector.h"
#include "idispatchable.h"

using namespace Commands;
using namespace Commands::Core;
//-----------------------------------------------------------------------------
GetData::GetData()
: dispatchable(0l)
, dataId(-1)
, queueId(-1)
, queueDefined(false)
{}
//-----------------------------------------------------------------------------
GetData::GetData(IDispatchable* dispatchable, const int dataId)
: dispatchable(dispatchable)
, dataId(dataId)
, queueId(-1)
, queueDefined(false)
{}
//-----------------------------------------------------------------------------
GetData::GetData(IDispatchable* dispatchable, const int queueId, const int dataId)
: dispatchable(dispatchable)
, dataId(dataId)
, queueId(queueId)
, queueDefined(true)
{}
//-----------------------------------------------------------------------------
float GetData::Execute(const float seconds)
{
    if(dispatchable)
    {
        if(queueDefined)
        {
            value = dispatchable->GetData(queueId, dataId);
        }
        else
        {
            value = dispatchable->GetData(dataId);
        }
    }
    return 0.0;
}
//-----------------------------------------------------------------------------
bool GetData::IsFinished(void) const
{
    return true;
}
//-----------------------------------------------------------------------------
GetData::~GetData()
{
}
//-----------------------------------------------------------------------------
GetData* GetData::Copy(void) const
{
    if(queueDefined)
    {
        return new GetData(dispatchable, queueId, dataId);
    }
    else
    {
        return new GetData(dispatchable, dataId);
    }
}
//-----------------------------------------------------------------------------
void GetData::Accept(Core::Inspector& inspector)
{
    inspector.Inspect(*this);
}
//-----------------------------------------------------------------------------
Add::Add(float first, float second)
: first(new Value(first))
, second(new Value(second))
{}
//-----------------------------------------------------------------------------
Add::Add(const Value& first, const Value& second)
: first(first.Copy())
, second(second.Copy())
{}
//-----------------------------------------------------------------------------
Add::Add(const Value& first, const float second)
: first(first.Copy())
, second(new Value(second))
{}
//-----------------------------------------------------------------------------
Add::Add(const float first, const Value& second)
: first(new Value(first))
, second(second.Copy())
{}
//-----------------------------------------------------------------------------
float Add::Execute(const float seconds)
{
    const float firstConsumedSeconds = first->Execute(seconds);
    const float secondConsumedSeconds = second->Execute(seconds);
    value = first->GetValue() + second->GetValue();
    return std::max(firstConsumedSeconds, secondConsumedSeconds);
}
//-----------------------------------------------------------------------------
bool Add::IsFinished(void) const
{
    return first->IsFinished() && second->IsFinished();
}
//-----------------------------------------------------------------------------
Add::~Add()
{
    delete first;
    delete second;
}
//-----------------------------------------------------------------------------
Add* Add::Copy(void) const
{
    return new Add(*first, *second);
}
//-----------------------------------------------------------------------------
void Add::Accept(Core::Inspector& inspector)
{
    inspector.Inspect(*this);
}
//-----------------------------------------------------------------------------
Uniform::Uniform(float start, float speed)
{
    assert(speed != 0.0);

    this->start = start;
    this->speed = speed;
    this->end   = (speed > 0.0) ? std::numeric_limits<float>::max() : -std::numeric_limits<float>::max();
    this->time  = fabs((end - start) / speed);
}

Uniform::Uniform(float start, float speed, float end)
{
    assert(speed != 0.0);

    this->start = start;
    this->speed = speed;
    this->end   = end;
    this->time  = fabs((end - start) / speed);
}

Uniform::Uniform(float start, float speed, float end, float time, Parameter parameter)
{
    assert(time > 0.0);
    switch(parameter)
    {
        case START:
            start  = end - (speed * time);
        break;

        case END:
            end  = start + (speed * time);
        break;

        case SPEED:
            speed  = ((end - start) / time);
        break;
    }
}

//-----------------------------------------------------------------------------
Accelerated::Accelerated(float start, float speed, float acceleration)
{
    this->start        = start;
    this->speed        = speed;
    this->acceleration = acceleration;
    this->end          = (acceleration > 0.0) ? std::numeric_limits<float>::max() : -std::numeric_limits<float>::max();
//    this->time =
}

Accelerated::Accelerated(float start, float speed, float acceleration, float end)
{
    this->start        = start;
    this->speed        = speed;
    this->acceleration = acceleration;
    this->end          = end;
//    this->time =
}

Accelerated::Accelerated(float start, float speed, float acceleration, float end, float time, Parameter parameter)
{
    this->start        = start;
    this->speed        = speed;
    this->acceleration = acceleration;
    this->end          = end;
    this->time         = time;

    switch(parameter)
    {
        case START:
            start  = end - (speed * time) - ((acceleration * time * time) / 2.0f);
        break;

        case END:
            end  = start + (speed * time) + ((acceleration * time * time) / 2.0f);
        break;

        case SPEED:
            speed  = ((end - start) / time) - (acceleration * time / 2.0f);
        break;

        case ACCELERATION:
            assert(time > 0.0f);
            acceleration  = (2.0f * ((end - start) - speed * time)) / (time * time);
        break;
    }
}
//-----------------------------------------------------------------------------
Decelerated::Decelerated(float start, float acceleration, float end)
{
    assert(acceleration != 0.0);
    this->start        = start;
    this->speed        = acceleration >= 0.0 ? -sqrt(2 * fabs(acceleration * (end - start))) - 0.5 : sqrt(2 * fabs(acceleration * (end - start))) + 0.5;
    this->acceleration = acceleration;
    this->end          = end;
    this->time         = fabs(this->speed / this->acceleration);
}

Decelerated::Decelerated(float start, float speed, float acceleration, float end, Parameter parameter)
{
    assert(acceleration != 0.0);

    printf("UNRELEASED METHOD\n");
    assert(false);

    this->start        = start;
    this->speed        = speed;
    this->acceleration = acceleration;
    this->end          = end;
    this->time         = fabs(this->speed / this->acceleration);
}
//-----------------------------------------------------------------------------
ChangeValue::ChangeValue(const KinematicLaw& law)
: start(law.GetStart())
, speed(law.GetSpeed())
, end(law.GetEnd())
, time(law.GetTime())
, acceleration(law.GetAcceleration())
, parameter(NOPARAMETER)
, elapsedTime(0.0)
, isFirstCall(true)
, isFinished(false)
{
    Value::value = 0.0f;
}
//-----------------------------------------------------------------------------
ChangeValue::ChangeValue(float start, float speed)
: start(start)
, speed(speed)
, end((speed > 0.0) ? std::numeric_limits<float>::max() : -std::numeric_limits<float>::max())
, time(0.0)
, acceleration(0)
, parameter(NOPARAMETER)
, elapsedTime(0.0)
, isFirstCall(true)
, isFinished(false)
{
    Init();
}
//-----------------------------------------------------------------------------
ChangeValue::ChangeValue(float start, float speed, float end)
: start(start)
, end(end)
, time(0.0)
, speed(speed)
, acceleration(0)
, parameter(NOPARAMETER)
, elapsedTime(0.0)
, isFirstCall(true)
, isFinished(false)
{
    Init();
}
//-----------------------------------------------------------------------------
ChangeValue::ChangeValue(float start, float speed, float acceleration, float end)
: start(start)
, end(end)
, time(0.0)
, speed(speed)
, acceleration(acceleration)
, parameter(NOPARAMETER)
, elapsedTime(0.0)
, isFirstCall(true)
, isFinished(false)
{
    Init();
}
//-----------------------------------------------------------------------------
ChangeValue::ChangeValue(float start, float speed, float acceleration, float end, float time, Parameter parameter)
: start(start)
, end(end)
, time(time)
, speed(speed)
, acceleration(acceleration)
, parameter(parameter)
, elapsedTime(0.0)
, isFirstCall(true)
, isFinished(false)
{
    Init();
}
//-----------------------------------------------------------------------------
void ChangeValue::Init(void)
{
    Value::value = 0.0f;

    switch(parameter)
    {
        case START:
            start  = end - (speed * time) - ((acceleration * time * time) / 2.0f);
        break;

        case END:
            end  = start + (speed * time) + ((acceleration * time * time) / 2.0f);
        break;

        case SPEED:
            speed  = ((end - start) / time) - (acceleration * time / 2.0f);
        break;

        case ACCELERATION:
            acceleration  = (2.0f * ((end - start) - speed * time)) / (time * time);
        break;
    }
}
//-----------------------------------------------------------------------------
float ChangeValue::Execute(const float seconds)
{
    if(isFirstCall)
    {
        Value::value = 0.0f;
        elapsedTime = 0.0f;
        isFirstCall = false;
        isFinished = false;
    }

    const float currentTime = elapsedTime + seconds;

    float currentValue = end;

    try
    {
        currentValue = start + speed * currentTime + acceleration * currentTime * currentTime / 2;
    }
    catch(std::runtime_error& error)
    {
        std::cerr << "exception caught: " << error.what() << std::endl;
        currentValue = end;
    }
    const float S = fabs(end - start);
    const float currentDistance = fabs(currentValue - start);

//    printf("S = %f\n", S);
//    printf("currentDistance = %f\n", currentDistance);

    if((S - currentDistance) <= 0.0)
    {
        Value::value = end;
        float shouldConsumeTime = 0;

        if(acceleration != 0.0)
        {
            shouldConsumeTime = (-speed + sqrt(speed * speed - 4 * (acceleration / 2 * (start - S)))) / acceleration;
        }
        else
        {
            shouldConsumeTime = fabs(S / speed);
        }

        isFinished = true;
        isFirstCall = true;

        float retval = shouldConsumeTime - elapsedTime;

        return retval;
    }
    else
    {
        Value::value = currentValue;

        elapsedTime += seconds;
        return seconds;
    }
}
//-----------------------------------------------------------------------------
bool ChangeValue::IsFinished(void) const
{
    return isFinished;
}
//-----------------------------------------------------------------------------
ChangeValue::~ChangeValue()
{}
//-----------------------------------------------------------------------------
ChangeValue* ChangeValue::Copy(void) const
{
    return new ChangeValue(start, speed, acceleration, end, time, parameter);
}
//-----------------------------------------------------------------------------
void ChangeValue::Accept(Core::Inspector& inspector)
{
    inspector.Inspect(*this);
}
//-----------------------------------------------------------------------------
Sine::Sine(float angle)
: angle(new Value(angle))
, magnitude(new Value(1.0))
{}
//-----------------------------------------------------------------------------
Sine::Sine(const Value& angle)
: angle(angle.Copy())
, magnitude(new Value(1.0))
{}
//-----------------------------------------------------------------------------
Sine::Sine(const Value& angle, const float swing)
: angle(angle.Copy())
, magnitude(new Value(swing))
{}
//-----------------------------------------------------------------------------
Sine::Sine(const Value& angle, const Value& swing)
: angle(angle.Copy())
, magnitude(swing.Copy())
{}
//-----------------------------------------------------------------------------
float Sine::Execute(const float seconds)
{
    const float angleConsumedSeconds = angle->Execute(seconds);
    const float swingConsumedSeconds = magnitude->Execute(seconds);
    value = magnitude->GetValue() * sin(angle->GetValue() * 3.14159265 / 180.0);
    return std::max(angleConsumedSeconds, swingConsumedSeconds);
}
//-----------------------------------------------------------------------------
bool Sine::IsFinished(void) const
{
    return angle->IsFinished() && magnitude->IsFinished();
}
//-----------------------------------------------------------------------------
Sine::~Sine()
{
    delete angle;
    delete magnitude;
}
//-----------------------------------------------------------------------------
Sine* Sine::Copy(void) const
{
    return new Sine(*angle, *magnitude);
}
//-----------------------------------------------------------------------------
void Sine::Accept(Core::Inspector& inspector)
{
    inspector.Inspect(*this);
}
//-----------------------------------------------------------------------------
Delta::Delta(const Value& _variable)
: variable(_variable.Copy())
, previousValue(variable->GetValue())
{}
//-----------------------------------------------------------------------------
float Delta::Execute(const float seconds)
{
    const float consumedSeconds = variable->Execute(seconds);
    value = variable->GetValue() - value;
    return consumedSeconds;
}
//-----------------------------------------------------------------------------
bool Delta::IsFinished(void) const
{
    return variable->IsFinished();
}
//-----------------------------------------------------------------------------
Delta::~Delta()
{
    delete variable;
}
//-----------------------------------------------------------------------------
Delta* Delta::Copy(void) const
{
    return new Delta(*variable);
}
//-----------------------------------------------------------------------------
void Delta::Accept(Core::Inspector& inspector)
{
    inspector.Inspect(*this);
}
//-----------------------------------------------------------------------------
